#ifndef POSE_PUB_PUBLISHER_H
#define POSE_PUB_PUBLISHER_H

#include <chrono>
#include <memory>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/utils.h"
#include "tf2_ros/transform_broadcaster.h"
#include <opencv2/opencv.hpp>

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class MinimalPublisher : public rclcpp::Node {
public:
    MinimalPublisher()
        : Node("cam_pose")
        , count_(0)
    {
        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
    }

    inline void publish(cv::Point3f &t, cv::Point3f &r)
    {
        count_++;

        // 发布
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = this->now();
        transform.header.frame_id = "camera";
        transform.child_frame_id = "world";
        // Fill in transform.transform.translation
        transform.transform.translation.x = t.x;
        transform.transform.translation.y = t.y;
        transform.transform.translation.z = t.z;
        // Fill in transform.transform.rotation
        tf2::Quaternion quatern;
        quatern.setRPY(r.x, r.y, r.z);
        quatern.normalize();
        transform.transform.rotation.x = quatern.x();
        transform.transform.rotation.y = quatern.y();
        transform.transform.rotation.z = quatern.z();
        transform.transform.rotation.w = quatern.w();
        tf_broadcaster_->sendTransform(transform);

        RCLCPP_INFO(this->get_logger(), "Publishing: '%ld'", count_);
    }

private:
    rclcpp::TimerBase::SharedPtr timer_;
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    size_t count_;
};


#endif